/*
 * Ken Anderson, 2009
 */

#ifndef SEARCH_H
#define SEARCH_H

#include "Graph.h"
#include "Heuristic.h"
#include "hash_map.h"
#include "util.h"
#include <queue>
#include <vector>

#define NUM_NODES_EXPANDED_PER_ITERATION 7		// Visualization purposes only.

class Heuristic;

// Search routines find the lowest cost path from a start node to a goal node.
// The cost of a path is the sum of all the edge weights along the path.
// All search routines must derive from this class.
class Search
{
protected:

	// Closed list elements.  Most importantly - this stores the g-value and the solution path.
	// The node pointer is the hash key, don't need to store it.
	struct ClosedListElement {
		float mGValue;
		unsigned char mClosedCount;
		Edge* mSolutionPathEdge;
		ClosedListElement() :
			mGValue(99999.0),
			mClosedCount(0),
			mSolutionPathEdge(NULL)
		{ }
		ClosedListElement(float gValue, unsigned char closedCount, Edge* solPathEdge) :
			mGValue(gValue),
			mSolutionPathEdge(solPathEdge)
		{ }
	};
	
	// Structures for Open list - stores Node pointer and f value (g+h)
	struct OpenListElement {
		Node* mNode;
		Edge* mInEdge;	// Used to reconstruct optimal path
		float mFValue;	// Used for priority
		float mGValue;	// Passed to the closed list
		OpenListElement() : 
			mNode(NULL), mInEdge(NULL), mFValue(0.0), mGValue(0.0)
		{ }
		OpenListElement(Node* node, Edge* inEdge, float fValue, float gValue)  :
			mNode(node), 
			mInEdge(inEdge),
			mFValue(fValue),
			mGValue(gValue)
		{ }
	};
	struct OpenListCompare {
		bool operator()(const OpenListElement &lhs, const OpenListElement &rhs) {
			if( aboutEqualTo(lhs.mFValue, rhs.mFValue) )	// breaking ties. larger g gets priority.
				return lhs.mGValue < rhs.mGValue;
			else											// Normal operation - smaller f gets priority
				return lhs.mFValue > rhs.mFValue;
		}
	};
	
	// Member variables
	Graph* mGraph;
	Heuristic * mHeuristic;
	Node* mStart;
	Node* mGoal;
	float mBestCost;
	std::vector<Edge*> mMinimalPath;
  HashMap<const Node*,ClosedListElement> mClosedList;
	std::priority_queue<OpenListElement, std::vector<OpenListElement>, OpenListCompare> mOpenList;
	int mNumExpanded;
	unsigned int mMaxSizeOpenList;
	bool mSearchRequired;

public:
	Search( Graph* graph, Heuristic * heuristic );
	virtual ~Search();

	// Use these during search.  The derived search classes must implement these functions.
	virtual void Init(Node* start, Node* goal);	// Initialize to prepare for search.
	virtual float FindMinimalPath() = 0;		// Search until solution is found.  Returns cost of minimal path.
	virtual bool IterateSearch() = 0;			// Expand up to a certain number of nodes.  Return TRUE if seach successfully finished.
	
	// Use these methods after the search is finished to get the interesting stats.
	unsigned int MinimalPathLength() { return mMinimalPath.size(); };
	Edge * MinimalPathEdge(unsigned int index) { return mMinimalPath.at(index); };
	bool IsClosed(Node* node) { return ( mClosedList.find(node) != mClosedList.end() ); };
	int GetExpansionCount(Node* node) { return mClosedList[node].mClosedCount; };
	float GetGVal( Node* node ) { return mClosedList[node].mGValue; };
	int NumExpandedNodes() {return mNumExpanded; };
	unsigned int GetMaxOpenListSize() {return mMaxSizeOpenList; };

	// Getters
	Graph * GetGraph() { return mGraph; };
	Node  * GetStart() { return mStart; };
	Node  * GetGoal() { return mGoal; };
	Heuristic * GetHeuristic() { return mHeuristic; };
};

// Iterative-Deepening A* is a depth-first search routine.
// Normally IDA* would not have an open / closed list,
// however, we rely on the open/closed list for visualization purposes.
// So we operate on these data structures to show the search progressing.
class IDAStar : public Search
{
protected:
	float mMaxDepth;
	Edge* mBestEdge;
	const static int mNumNodeExpansionsOnIterate=1;

	// Recursive, depth-first node expansion.
	float RecursiveSearch(Node* current, float depth);

public:
	IDAStar(Graph* graph, Heuristic* heuristic) : Search( graph, heuristic) {};
	~IDAStar() {};
	void Init(Node* start, Node* goal);
	float FindMinimalPath();
	bool IterateSearch();
};

// Classic A* search.
class AStar : public Search
{
	const static int mNumNodeExpansionsOnIterate=NUM_NODES_EXPANDED_PER_ITERATION;
	bool ExpandTopNode();
	void ReconstructSolution();
public:
	AStar( Graph* graph, Heuristic * heuristic ) : Search( graph, heuristic) {};
	~AStar() {};
	void Init(Node* start, Node* goal);
	float FindMinimalPath();
	bool IterateSearch();
	// RRA*-type searching.  Either stop when node is found, or continue search until graph is solved.
	float ResumeSearch(Node * node, bool stopWhenFound=true);
};


#endif
